#include "ustc_comdef.h"
#include "ustc_custom.h"
#include "ustc_hif.h"

void ustc_outh (USTC_U16 index, USTC_U16 data)
{
	USTC_U32 tmp;

	USTC_IO_LOCK(tmp);
	//send address
	outph(HIF_REG_ADDR, index);

	//send writen data
	outph(HIF_REG_DATA, data);
	USTC_IO_UNLOCK(tmp);
}

USTC_U16 ustc_inh (USTC_U16 index)
{
	USTC_U32 tmp;
	USTC_U16 val;

	USTC_IO_LOCK(tmp);
	//send address
	outph(HIF_REG_ADDR, index);
	
	//send writen data
	val=inph(HIF_REG_DATA);
	USTC_IO_UNLOCK(tmp);

	return val;
}

void ustc_reg_set (USTC_U16 index, USTC_U16 bits)
{
	USTC_U32 tmp;

	USTC_IO_LOCK(tmp);

	outph(HIF_REG_ADDR, index);
	
	bits|=inph(HIF_REG_DATA);
	
	outph(HIF_REG_DATA, bits);

	USTC_IO_UNLOCK(tmp);
}

void ustc_reg_clr (USTC_U16 index, USTC_U16 bits)
{
	USTC_U32 tmp;

	USTC_IO_LOCK(tmp);

	outph(HIF_REG_ADDR, index);
	
	bits=inph(HIF_REG_DATA)&(~bits);
	
	outph(HIF_REG_DATA, bits);

	USTC_IO_UNLOCK(tmp);
}

void ustc_io_outw (USTC_U16 index, USTC_U32 data)
{
	USTC_U32 tmp;
	
	USTC_IO_LOCK(tmp);
	
	// write start address low-16bit
	outph (HIF_REG_ADDR, HIF_AHB_ADDR); // offset 0x0004
	outph (HIF_REG_DATA, HIF_IO_CMD_W|index); // data 0x----

	// write data
	outph (HIF_IO_DATA, data & 0xffff);
	outph (HIF_IO_DATA, (data >>16));

	USTC_IO_UNLOCK(tmp);
}

void ustc_io_outw2 (USTC_U16 index, USTC_U32 data)
{
	// write start address low-16bit
	outph (HIF_REG_ADDR, HIF_AHB_ADDR); // offset 0x0004
	outph (HIF_REG_DATA, HIF_IO_CMD_W|index); // data 0x----

	// write data
	outph (HIF_IO_DATA, data & 0xffff);
	outph (HIF_IO_DATA, (data >>16));
}

USTC_U32 ustc_io_inw (USTC_U16 index)
{	
	USTC_U32 temp1, temp2; //avoid shift left error on 16-bit system
	USTC_U32 tmp;
	
	USTC_IO_LOCK(tmp);

	// read start address low-16bit
	outph (HIF_REG_ADDR, HIF_AHB_ADDR); // offset 0x0004
	outph (HIF_REG_DATA, HIF_IO_CMD_R|index); // data 0x----

	HIF_IO_DELAY();	//needed by HREADY

	// read data
	temp2= inph (HIF_IO_DATA);
	temp1= inph (HIF_IO_DATA);

	USTC_IO_UNLOCK(tmp);

	return (temp1<<16) | temp2;
}

USTC_U32 ustc_io_inw2 (USTC_U16 index)
{	
	USTC_U16 temp1, temp2;
	
	// read start address low-16bit
	outph (HIF_REG_ADDR, HIF_AHB_ADDR); // offset 0x0004
	outph (HIF_REG_DATA, HIF_IO_CMD_R|index); // data 0x----

	HIF_IO_DELAY();	//needed by HREADY

	// read data
	temp2= inph (HIF_IO_DATA);
	temp1= inph (HIF_IO_DATA);

	return (temp1<<16) | temp2;
}

void ustc_io_outb (USTC_U16 index, USTC_U8 data)
{
	USTC_U16 v=data;
	USTC_U32 tmp;

	((USTC_U8*)&v)[1]=data;
	
	USTC_IO_LOCK(tmp);

	// write start address low-16bit
	outph (HIF_REG_ADDR, HIF_AHB_ADDR); // offset 0x0004
	outph (HIF_REG_DATA, HIF_IO_CMD_W|HIF_IO_BYTE|index); // data 0x----

	// write data
	outph (HIF_IO_DATA, v);
	outph (HIF_IO_DATA, v);

	USTC_IO_UNLOCK(tmp);
}


USTC_U8 ustc_io_inb (USTC_U16 index)
{	
	USTC_U16 temp[2];
	USTC_U32 tmp;

	USTC_IO_LOCK(tmp);
//	__asm{
//		MRS tmp, CPSR
//		AND tmp2, tmp, #0x80
//		ORR tmp, tmp, #0x80
//		MSR CPSR_c, tmp
//	}
	// read start address low-16bit
	outph (HIF_REG_ADDR, HIF_AHB_ADDR); // offset 0x0004
	outph (HIF_REG_DATA, HIF_IO_CMD_R|HIF_IO_BYTE|index); // data 0x----

	HIF_IO_DELAY();	//needed by HREADY
	
	// read data
	temp[0]= inph (HIF_IO_DATA);
	temp[1]= inph (HIF_IO_DATA);
//	__asm{
//		MRS tmp, CPSR
//		BIC tmp, tmp, #0x80
//		ORR tmp, tmp, tmp2
//		MSR CPSR_c, tmp
//	}
	USTC_IO_UNLOCK(tmp);

	switch (index&3)
	{
		case 1:
			return temp[0]>>8;
		case 2:
			return temp[1];
		case 3:
			return temp[1]>>8;
		default:
			return temp[0];
	}
}

void ustc_io_burst_inw(USTC_U16 index, USTC_U32* buf)
{	
	USTC_U32 tmp;
	USTC_U16 *p=(USTC_U16*)buf;
	
	USTC_IO_LOCK(tmp);

	outph(HIF_REG_ADDR, HIF_IO_BURST_LEN);
	outph(HIF_REG_DATA, 8);
	// read start address low-16bit
	outph(HIF_REG_ADDR, HIF_AHB_ADDR); // offset 0x0004
	outph(HIF_REG_DATA, HIF_IO_CMD_R|HIF_IO_CMD_BURST|index); // data 0x----

	HIF_IO_DELAY();	//needed by HREADY

	// read data
	*p++= inph(HIF_IO_DATA);
	*p++= inph(HIF_IO_DATA);

	*p++= inph(HIF_IO_DATA);
	*p++= inph(HIF_IO_DATA);

	*p++= inph(HIF_IO_DATA);
	*p++= inph(HIF_IO_DATA);

	*p++= inph(HIF_IO_DATA);
	*p++= inph(HIF_IO_DATA);

	*p++= inph(HIF_IO_DATA);
	*p++= inph(HIF_IO_DATA);

	*p++= inph(HIF_IO_DATA);
	*p++= inph(HIF_IO_DATA);

	*p++= inph(HIF_IO_DATA);
	*p++= inph(HIF_IO_DATA);

	*p++= inph(HIF_IO_DATA);
	*p++= inph(HIF_IO_DATA);

	USTC_IO_UNLOCK(tmp);
}

void ustc_io_burst_outw(USTC_U16 index, USTC_U32 * buf)
{	
	USTC_U32 tmp;
	USTC_U16 *p=(USTC_U16*)buf;
	
	USTC_IO_LOCK(tmp);

	outph(HIF_REG_ADDR, HIF_IO_BURST_LEN);
	outph(HIF_REG_DATA, 8);
	// read start address low-16bit
	outph(HIF_REG_ADDR, HIF_AHB_ADDR); // offset 0x0004
	outph(HIF_REG_DATA, HIF_IO_CMD_W|HIF_IO_CMD_BURST|index); // data 0x----

	// read data
	outph(HIF_IO_DATA, *p++);
	outph(HIF_IO_DATA, *p++);

	outph(HIF_IO_DATA, *p++);
	outph(HIF_IO_DATA, *p++);

	outph(HIF_IO_DATA, *p++);
	outph(HIF_IO_DATA, *p++);

	outph(HIF_IO_DATA, *p++);
	outph(HIF_IO_DATA, *p++);

	outph(HIF_IO_DATA, *p++);
	outph(HIF_IO_DATA, *p++);

	outph(HIF_IO_DATA, *p++);
	outph(HIF_IO_DATA, *p++);
	
	outph(HIF_IO_DATA, *p++);
	outph(HIF_IO_DATA, *p++);

	outph(HIF_IO_DATA, *p++);
	outph(HIF_IO_DATA, *p++);

	USTC_IO_UNLOCK(tmp);
}

